package muurvolger;
import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class muurvolger{

	int ultraDistance;
	static int distance = 20;
	int difference;
	Pilot p;
	
	int angle = 45;
	SimpleNavigator nav;
	boolean forward;
	int left;
	float side;

	class SonicThread implements Runnable {
		public void run(){
			final UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);
			while(true){
				int[] distances;
				distances = new int[10];
				// Omwille van onverklaarbare pieken in de meetwaarden van 255
				// wordt een array gebruikt van 10 meetwaarden.
				
				for(int i =0; i<distances.length;i++){
					distances[i] = sonic.getDistance(); 
					try{
						Thread.sleep(2);
					}catch(InterruptedException e){
					}
				}
				
				// Van de bekomen 10 meetwaarden wordt dan de kleinste berekend,
				// gezien die relatief gezien het meest nauwkeurig zal zijn.
				int smallest = distances[0];
				for(int i = 1; i<distances.length;i++){
					if(distances[i]<=smallest)
						smallest = distances[i];;
				}
				ultraDistance = smallest;
			}
		}
	}
	
	class Move implements Runnable {
		public void run(){
			while(true){
				// Voorwaarts rijden ("oneindig" lang) indien dit toegelaten wordt
				if(forward)
					p.travel(10000,true);
				// Anders een boog maken in een richting die bepaald wordt door 
				// de kant waar de muur staat, en hoever de muur is.
				else 
					p.steer(left*side*35,angle,true);
			}
		}
	}
	
	public static void main(String[] args)
	{
		// Nieuwe muurvolger starten.
		new muurvolger().go();
	}

	private void go()
	{
		// Alles initialiseren.
		p = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C,true);
		Thread t = new Thread(new SonicThread());
		Thread m = new Thread(new Move());
		SetWallSide();
		p.setTurnSpeed(40);
		t.start();
		m.start();
		forward = true;
		while (true) wallFollow();

	}

	// De kant waar de muur staat invoeren.
	private void SetWallSide() {
		int id = Button.waitForPress();
		if(id == Button.ID_LEFT){
			Motor.B.rotate(angle);
			left = -1;
		}
		else{
			Motor.B.rotate(-angle);
			left = 1;
		}
	}
	
	// Hier gebeurt de beslissing of de muur te dicht/ver is, of dat we rechtdoor
	// mogen blijven rijden.
	private void wallFollow(){
		try{
			int ultra = ultraDistance;
			int currentDis = (int) (Math.cos(angle)*ultra);
			System.out.println("CurrentDis = " + currentDis);
//			if(currentDis < 0.8*distance){
//				side = 1;
//				// Als de muur echt te dichtbij komt, sneller wegdraaien.
//				if(currentDis < .5*distance){
//					side = 1.5f;
//				}
//				forward = false;
//			}
//			else if(currentDis > 1.2*distance){
//				side = -0.8f;
//				// Als de muur "verdwijnt" of een scherpe buitenbocht maakt, sneller
//				// meedraaien.
//				if(currentDis > 2.5*distance){
//					side = -1.3f;
//				}
//				forward = false;
//			}
			if(currentDis < 0.8*distance){
				side = (float)Math.sqrt((((float) distance)/((float)currentDis)));
				// Als de muur echt te dichtbij komt, sneller wegdraaien.
				if(currentDis < .5*distance){
					side = 1.5f;
				}
				forward = false;
			}
			else if(currentDis > 1.2*distance){
				side =-(float)(Math.sqrt((((float) currentDis)/((float)distance))*0.9);
				// Als de muur "verdwijnt" of een scherpe buitenbocht maakt, sneller
				// meedraaien.
				if(currentDis > 1.9*distance){
					side = -1.3f;
				}
				forward = false;
			}
			else{
				forward = true;
			}

		}
		catch(Exception e){
			System.out.println("EXCEPTION" + e.toString());
			try {
				Thread.sleep(2000);
			} catch (InterruptedException e1) {

			}
		}
	}

}
